import serial
import time
import math
import numpy as np
from matrixTools import rotation_matrix_to_euler_angles,trans2matrix

class Dummy:
    def __init__(self,port,baudrate=115200,timeout=1):
        self.ser = serial.Serial()
        self.ser.port = port
        self.ser.baudrate = baudrate
        self.ser.timeout = timeout
        self.curAngles = [0.0,0.0,0.0,0.0,0.0,0.0]
        self.curPos = [0.0,0.0,0.0,0.0,0.0,0.0]
        self.tgtPos = [0.0,0.0,0.0,0.0,0.0,0.0]
        self.arrived_state = True
        # 工具坐标系变换
        self.T6t = np.eye(4)
        self.T6t[0,3] = -10
        self.T6t[2,3] = 115
        self.Tb6_cur = None

    def open_serial(self):
        try:
            self.ser.open()
            print("serial open success")
        except Exception as e:
            print("serial open error")
    def close_serial(self):
        if self.ser.is_open:
            self.ser.close()
            print("serial close")
    
    # 读取六个关节的角度值
    def readJoints(self):
        COMMAND = "#GETJPOS\r\n"
        if self.ser.in_waiting > 0:
            all_data = self.ser.read_all()
        self.ser.write(COMMAND.encode('utf-8'))
        time.sleep(0.01)
        raw_data = self.ser.readline()
        if raw_data:
            # 去除回车换行符并解码
            decoded_data = raw_data.decode('utf-8').rstrip('\r\n')
            #print(f"收到响应: {decoded_data}")
            # 使用空格分割响应
            parts = decoded_data.split()
            if len(parts) != 7:
                print("readJoints error!")
                return False
            for i in range(len(parts)):
                if i == 0:
                    continue
                self.curAngles[i-1] = parts[i]

    # 读取机械臂当前的位姿 循环读取的速度 0.000964秒
    def readPos(self, timeout=1.0):
        COMMAND = "##GETLPOS\r\n"
        self.ser.reset_input_buffer()
        self.ser.write(COMMAND.encode('utf-8'))
        self.ser.flush()
        start_time = time.time()
        while (time.time() - start_time) < timeout:
            if self.ser.in_waiting > 0:
                raw_data = self.ser.readline()
                if raw_data:
                    # 去除回车换行符并解码
                    decoded_data = raw_data.decode('utf-8').rstrip('\r\n')
                    #print(f"收到响应: {decoded_data}")
                    # 使用空格分割响应
                    parts = decoded_data.split()
                    if len(parts) != 7:
                        print("####################readPos error!")
                        print(f"收到响应: {decoded_data}")
                        return False
                    for i in range(len(parts)):
                        if i == 0:
                            continue
                        try:
                            self.curPos[i-1] = float(parts[i])
                        except:
                            print(f"收到响应: {decoded_data}")
                            print("readPos float error!")
                    #print(f"readPos: {self.curPos}")
                    #计算当前的位姿
                    Tb6 = trans2matrix(self.curPos)
                    self.Tb6_cur = Tb6
                    Tbt = Tb6 @ self.T6t
                    return Tbt
                else:
                    return None   


    # 使能机械臂
    def EnableDummy(self,flag):
        if flag == True:
            COMMAND = "!START\r\n"
        else:
            COMMAND = "!DISABLE\r\n"
        self.ser.write(COMMAND.encode('utf-8'))
        time.sleep(0.01)
        raw_data = self.ser.readline()
        if raw_data:
            # 去除回车换行符并解码
            decoded_data = raw_data.decode('utf-8').rstrip('\r\n')
            print(f"收到响应: {decoded_data}")

    # 机械臂home位
    def moveHome(self):
        COMMAND = "!HOME\r\n"
        self.ser.write(COMMAND.encode('utf-8'))
        time.sleep(0.01)
        raw_data = self.ser.readline()
        if raw_data:
            # 去除回车换行符并解码
            decoded_data = raw_data.decode('utf-8').rstrip('\r\n')
            print(f"收到响应: {decoded_data}")
    
    # 设置PID
    def setPID(self):
        # #SET_DCE_KD 6 1000
        COMMAND = "#SET_DCE_KD 6 1000\r\n"
        self.ser.write(COMMAND.encode('utf-8'))
        raw_data = self.ser.readline()
        if raw_data:
            # 去除回车换行符并解码
            decoded_data = raw_data.decode('utf-8').rstrip('\r\n')
            print(f"收到响应: {decoded_data}")


    # 机械臂折叠位、休息位、RESET位
    def moveRESET(self):
        COMMAND = "!RESET\r\n"
        self.ser.write(COMMAND.encode('utf-8'))
        time.sleep(0.01)
        raw_data = self.ser.readline()
        if raw_data:
            # 去除回车换行符并解码
            decoded_data = raw_data.decode('utf-8').rstrip('\r\n')
            print(f"收到响应: {decoded_data}")
    
    # 控制移动到指定角度
    def moveAngles(self,joint_angles,speed=20):
        COMMAND = f"&{joint_angles[0]:.2f},{joint_angles[1]:.2f},{joint_angles[2]:.2f},{joint_angles[3]:.2f},{joint_angles[4]:.2f},{joint_angles[5]:.2f},{speed:.2f}\r\n"
        print(COMMAND)
        self.ser.write(COMMAND.encode('utf-8'))
        time.sleep(0.01)
        all_data = self.ser.read_all()


    # 更新runflag，看pos有无到达
    def update_arrivedState(self):
        threshold = 2
        self.readPos()
        time.sleep(0.02)
        self.arrived_state = all(abs(a - b) < threshold for a, b in zip(self.curPos[:3], self.tgtPos[:3]))
        #print("#######update_arrivedState:")
        #print(self.curPos)
        #print(self.tgtPos)
        
        return self.arrived_state

    # 控制移动到指定位姿 @222,0,307,0,90,0
    def move2Pos(self,x,y,z,Rx,Ry,Rz,speed=10):
        self.tgtPos = [x,y,z,Rx,Ry,Rz]
        self.arrived_state = False

        # 自适应速度控制
        if speed < 0:
            Tbt = self.readPos()
            MIN_DISTANCE = 5    # 最小距离（对应最低速度）
            MAX_DISTANCE = 50   # 最大距离（对应最高速度）
            MIN_SPEED = 10       # 最低速度
            MAX_SPEED = 30      # 最高速度
            if Tbt is not None:
                dx = x - self.Tb6_cur[0,3]
                dy = y - self.Tb6_cur[1,3]
                dz = z - self.Tb6_cur[2,3]
                distance = math.sqrt(dx**2 + dy**2 + dz**2)
                # 线性调速计算
                if distance <= MIN_DISTANCE:
                    # 距离小于最小阈值，使用最低速度
                    speed = MIN_SPEED
                elif distance >= MAX_DISTANCE:
                    # 距离大于最大阈值，使用最高速度
                    speed = MAX_SPEED
                else:
                    # 中间距离，线性插值计算速度
                    speed = MIN_SPEED + (MAX_SPEED - MIN_SPEED) * (distance - MIN_DISTANCE) / (MAX_DISTANCE - MIN_DISTANCE)

        #COMMAND = f"@{x},{y},{z},{Rx},{Ry},{Rz},{speed}\r\n"
        COMMAND = f"@{x:.2f},{y:.2f},{z:.2f},{Rx:.2f},{Ry:.2f},{Rz:.2f},{speed:.2f}\r\n"
        print(COMMAND)
        self.ser.write(COMMAND.encode('utf-8'))
        time.sleep(0.01)
        # raw_data = self.ser.readline()
        # if raw_data:
        #     # 去除回车换行符并解码
        #     decoded_data = raw_data.decode('utf-8').rstrip('\r\n')
        #     print(f"收到响应: {decoded_data}")
        all_data = self.ser.read_all()


    # 控制移动到指定位姿T
    def move2T(self,Tb6,speed=20):
        eula = rotation_matrix_to_euler_angles(Tb6)
        print(f"move2T:{Tb6[0,3]},{Tb6[1,3]},{Tb6[2,3]},{eula[0]},{eula[1]},{eula[2]}")
        self.move2Pos(Tb6[0,3],Tb6[1,3],Tb6[2,3],
                      eula[0],eula[1],eula[2],
                      speed)
    
    # 控制移动到指定位姿，相对于位姿Tb6的Z轴上偏移bias_z处
    def move2T_bias(self,Tb6,bia_x,bia_y,bia_z,speed=20):
        T6u = np.eye(4)
        T6u[0,3] = bia_x
        T6u[1,3] = bia_y
        T6u[2,3] = bia_z
        Tbu = Tb6 @ T6u

        eula = rotation_matrix_to_euler_angles(Tbu)
        print(f"move2T_bias:{Tbu[0,3]},{Tbu[1,3]},{Tbu[2,3]},{eula[0]},{eula[1]},{eula[2]}")
        self.move2Pos(Tbu[0,3],Tbu[1,3],Tbu[2,3],
                        eula[0],eula[1],eula[2],
                        speed)


    # 等待到达
    def waitArrived(self):
        while True:
            isArrived = self.update_arrivedState()
            if isArrived:
                print(f"isArrived:{isArrived}")
                break

    # demo1：以pos为中心，画一个半径为radius的圆
    def drawCicle(self,pos,radius):
        """
        以指定位置为中心，绘制一个圆
        
        参数:
        pos: 圆心位置 [x, y, z]
        radius: 圆的半径
        """
        # 获取圆心坐标
        center_x, center_y, center_z = pos
        
        # 设置圆的采样点数
        num_points = 36  # 每10度一个点
        
        # 绘制圆
        for i in range(num_points):
            # 计算当前角度（弧度）
            angle = 2 * math.pi * i / num_points
            
            # 计算圆上的点坐标
            x = center_x + radius * math.cos(angle)
            y = center_y + radius * math.sin(angle)
            z = center_z  # 保持z坐标不变
            
            # 移动到圆上的点，保持姿态为 [0, 90, 0]
            self.move2Pos(x, y, z, 0, 180, 0)
            self.waitArrived()
            #print(f"x:{x}  ,y:{y},  z:{z}")
        
        # 回到起点，闭合圆
        self.move2Pos(center_x + radius, center_y, center_z, 0, 180, 0)
        self.waitArrived()



if __name__ == "__main__":
    print("--Dummy main test--")
    #dummy = Dummy('/dev/ttyACM0') #linux系统
    dummy = Dummy('COM10')         #windows系统
    dummy.open_serial()
    dummy.EnableDummy(True)
    # dummy.readPos()
    # dummy.setPID()
    #dummy.moveHome()
    #time.sleep(5)
    dummy.drawCicle([210,0,160],50)
